#include "GMath.h"
#include "GMatrix4x4.h"
#include "GIntersector.h"

namespace GMath
{


	GIntersector::GIntersector()
	{
	}

	GIntersector::GIntersector(const GIntersector& Other)
	{
	}

	GIntersector::~GIntersector()
	{
	}


	bool GIntersector::Intersect(const GRay2& Ray, const GTriangle2& Triangle)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GTriangle2& Triangle, float& T0)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GRectangleAligned& Rectangle)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GRectangleAligned& Rectangle, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GRectangleOriented& Rectangle)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GRectangleOriented& Rectangle, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GCircle& Circle)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay2& Ray, const GCircle& Circle, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GDisk& Disk)
	{
		float T0, T1;
		return Intersect(Ray, Disk, T0, T1);
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GDisk& Disk, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		GPlane Plane(Disk.Center, Disk.Normal);

		bool Result = Intersect(Ray, Plane, T0, T1);
	
		if (!Result)
		{
			return false;
		}

		GVector3 Point = Ray.FindPoint(T0);
		GVector3 Vec = Point - Disk.Center;

		float RR = Disk.Radius * Disk.Radius;

		return Vec.LenghtSqr() <= RR;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GPlane& Plane)
	{
		float T0, T1;
		return Intersect(Ray, Plane, T0, T1);
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GPlane& Plane, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		float NdotL = Plane.Normal.Dot(Ray.Direction);

		if (NdotL < G_ZERO_TRESHOLD)
		{
			return false;
		}

		GVector3 Vec = Plane.Point - Ray.Origin;
		T0 = Vec.Dot(Plane.Normal) / NdotL;
		T1 = T0;

		return T0 >= 0.0f;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GSphere& Sphere)
	{
		GVector3 L = Sphere.Center - Ray.Origin;
		float Tca = L.Dot(Ray.Direction);
    
		if (Tca < 0.0f)
		{
			return false;
		}
	
		float d2 = L.LenghtSqr() - Tca * Tca;

		return d2 < Sphere.Radius * Sphere.Radius;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GSphere& Sphere, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		GVector3 L = Sphere.Center - Ray.Origin;
		float Tca = L.Dot(Ray.Direction);
    
		if (Tca < 0.0f)
		{
			return false;
		}
	
		float RR = Sphere.Radius * Sphere.Radius;
		float DD = L.LenghtSqr() - Tca * Tca;

		if (DD > RR)
		{
			return false;
		}

		float Thc = SqrtF(RR - DD);

		T0 = Tca - Thc;
		T1 = Tca + Thc;

		return true;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GTriangle3& Triangle)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GTriangle3& Triangle, float& T0, float& T1)
	{
		//!++ YAZILACAK
		return false;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GBoxAligned& Box)
	{
		float T0, T1;
		return Intersect(Ray, Box, T0, T1);
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GBoxAligned& Box, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;

		for (GUInt32 I = 0; I < 3; ++I)
		{
			bool Parallel = EqualF(Ray.Direction[I], 0.0f);
			bool InsideSlab = (Ray.Direction[I] < Box.Min[I]) || (Ray.Direction[I] > Box.Max[I]);

			if (Parallel)
			{
				if (InsideSlab)
				{
					return false;
				}
			}
			else
			{
				float LocT0 = (Box.Min[I] - Ray.Origin[I]) / Ray.Direction[I];
				float LocT1 = (Box.Max[I] - Ray.Origin[I]) / Ray.Direction[I];

				if (LocT0 > LocT1)
				{
					Swap(LocT0, LocT1);
				}

				T0 = Max(LocT0, T0);
				T1 = Min(LocT1, T1);

				if (T0 > T1)
				{
					// box is missed
					return false;
				}
				if (T1 < 0)
				{
					// box is behind ray
					return false;
				}
			}
		}

		return true;
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GBoxOriented& Box)
	{
		float T0, T1;
		return Intersect(Ray, Box, T0, T1);
	}

	bool GIntersector::Intersect(const GRay3& Ray, const GBoxOriented& Box, float& T0, float& T1)
	{
		T0 = G_FLOAT_MIN;
		T1 = G_FLOAT_MAX;
	
		GVector3 HalfDim = Box.Dimension / 2.0f;
		GVector3 CenterVec = Box.Center - Ray.Origin;
		GVector3 Axises[3] = {Box.GetRight(), Box.GetUp(), Box.GetFront()};

		for (GUInt32 I = 0; I < 3; ++I)
		{
			float L = Axises[I].Dot(CenterVec);
			float A = Axises[I].Dot(Ray.Direction);

			bool Parallel = EqualF(A, 0.0f);
			bool InsideSlab = (L < -HalfDim[I]) || (L > HalfDim[I]);

			if (Parallel)
			{
				if (InsideSlab)
				{
					return false;
				}
			}
			else
			{
				float LocT0 = (L + HalfDim[I]) / A;
				float LocT1 = (L - HalfDim[I]) / A;
				if (LocT0 > LocT1)
				{
					Swap(LocT0, LocT1);
				}

				T0 = Max(LocT0, T0);
				T1 = Min(LocT1, T1);

				if (T0 > T1)
				{
					return false;
				}
				if (T1 < 0.0f)
				{
					return false;
				}
			}
		}

		return true;
	}

	bool GIntersector::Intersect(const GCircle& Circle, const GVector2& Point)
	{
		float RadiusSqr = Circle.Radius * Circle.Radius;
		float DistanceSqr =	Point.DistanceSqr(Circle.Center);

		return DistanceSqr <= RadiusSqr;
	}

	bool GIntersector::Intersect(const GCircle& Circle, const GCircle& Circle2)
	{
		float RadiusSumSqr = PowF(Circle.Radius + Circle2.Radius, 2.0f);
		float CenterDistSqr = Circle.Center.DistanceSqr(Circle2.Center);

		return RadiusSumSqr >= CenterDistSqr;
	}

	bool GIntersector::Intersect(const GRectangleAligned& Rectangle, const GCircle& Circle)
	{
		GVector2 CenterDist = Circle.Center - Rectangle.GetCenter();
		CenterDist.Absolute();

		GVector2 Dim = Rectangle.GetDimension();

		if (CenterDist.x > (Dim.x / 2.0f + Circle.Radius))
		{ 
			return false;
		}
		if (CenterDist.y > (Dim.y / 2.0f + Circle.Radius))
		{ 
			return false;
		}

		if (CenterDist.x <= (Dim.x / 2.0f))
		{
			return true;
		}
		if (CenterDist.y <= (Dim.y / 2.0f))
		{
			return true;
		}

		GVector2 CornerVec = CenterDist - Dim;
		float CornerDistSqr = CornerVec.Dot(CornerVec);

		return (CornerDistSqr <= (Circle.Radius * Circle.Radius));
	}

	bool GIntersector::Intersect(const GRectangleAligned& Rectangle, const GVector3& Point)
	{
		bool Right = Point.x > Rectangle.Max.x;
		bool Left = Point.x < Rectangle.Min.x;
		bool Up = Point.y > Rectangle.Max.y;
		bool Down = Point.y < Rectangle.Min.y;

		return !Right && !Left && !Up & !Down;
	}

	bool GIntersector::Intersect(const GRectangleAligned& Rectangle, const GRectangleAligned& Rectangle2)
	{
		bool XOverlap = (Rectangle.Min.x <= Rectangle2.Max.x) || 
						(Rectangle.Max.x >= Rectangle2.Min.x);
		bool YOverlap = (Rectangle.Max.y <= Rectangle2.Min.y) || 
						(Rectangle.Min.y >= Rectangle2.Max.y);
	
		return XOverlap && YOverlap;
	}

	bool GIntersector::Intersect(const GSphere& Sphere, const GBoxOriented& Box)
	{
		GVector3 Axises[3] = {Box.Right, Box.Up, Box.Front};

		GVector3 Point = Box.Center;
		GVector3 HalfDim = Box.Dimension / 2.0f;
		GVector3 D = Sphere.Center - Box.Center;

		for (GUInt32 I = 0; I < 3; ++I)
		{
			float Dist = Axises[I].Dot(D);
		
			Dist = Max(Dist,  HalfDim[I]);
			Dist = Min(Dist, -HalfDim[I]);

			Point += Dist * Axises[I];
		}

		GVector3 V = Point - Sphere.Center;
		return V.LenghtSqr() <= Sphere.Radius * Sphere.Radius;
	}

	bool GIntersector::Intersect(const GSphere& Sphere, const GBoxAligned& Box)
	{
		static const GVector3 Axises[3] = { GVector3::UnitX, GVector3::UnitY, GVector3::UnitZ };
		float MinDist = 0.0f;

		GVector3 BoxCenter = Box.GetCenter();
		GVector3 HalfDim = Box.GetDimension() / 2.0f;
		GVector3 D = Sphere.Center - BoxCenter;

		GVector3 Point = BoxCenter;
		for (GUInt32 I = 0; I < 3; ++I)
		{
			float Dist = Axises[I].Dot(D);

			Dist = Max(Dist,  HalfDim[I]);
			Dist = Min(Dist, -HalfDim[I]);

			Point += Dist * Axises[I];
		}

		GVector3 V = Point - Sphere.Center;
		return V.LenghtSqr() <= Sphere.Radius * Sphere.Radius;
	}

	bool GIntersector::Intersect(const GSphere& Sphere, const GSphere& Sphere2)
	{
		float CenterDistSqr = (Sphere.Center - Sphere2.Center).LenghtSqr();

		return CenterDistSqr <= PowF(Sphere.Radius + Sphere2.Radius, 2.0f);
	}

	bool GIntersector::Intersect(const GSphere& Sphere, const GVector3& Point)
	{
		float CenterDistSqr = (Point - Sphere.Center).LenghtSqr();

		return CenterDistSqr <= Sphere.Radius * Sphere.Radius;
	}

	bool GIntersector::Intersect(const GBoxOriented& Box, const GBoxOriented& Box2)
	{
		float epsilon = 1e-3f;

		GVector3 AxisesBox[3]  = {Box.Right, Box.Up, Box.Front};
		GVector3 AxisesBox2[3] = {Box2.Right, Box2.Up, Box2.Front};

		GMatrix4x4 r_mat = GMatrix4x4::Identity;
		for (GUInt32 I = 0; I < 3; ++I)
		{
			for (GUInt32 J = 0; J < 3; ++J)
			{
				r_mat[I, J] = AxisesBox[I].Dot(AxisesBox2[J]);
			}
		}

		GVector3 t = Box2.Center - Box.Center;
		t.x = AxisesBox[0].Dot(t);
		t.y = AxisesBox[1].Dot(t);
		t.z = AxisesBox[2].Dot(t);

		GMatrix4x4 abs_r_mat = GMatrix4x4::Identity;
		for (GUInt32 I = 0; I < 3; ++I)
		{
			for (GUInt32 J = 0; J < 3; ++J)
			{
				abs_r_mat[I, J] = AbsF(r_mat[I, J]) + epsilon;
			}
		}

		GVector3 lr = Box.Dimension / 2.0f;
		GVector3 rr = Box2.Dimension / 2.0f;

		// Test the three major axes of this OBB.
		for (GUInt32 I = 0; I < 3; ++I)
		{
			float ra = lr[I];
			float rb = rr[0] * abs_r_mat(I, 0) +  rr[1] * abs_r_mat(I, 1) + rr[2] * abs_r_mat(I, 2);
		
			if (AbsF(t[I]) > ra + rb) 
			{
				return false;
			}
		}

		// Test the three major axes of the OBB b.
		for (GUInt32 I = 0; I < 3; ++I)
		{
			float rb = rr[I];
			float ra = lr[0] * abs_r_mat(0, I) + lr[1] * abs_r_mat(1, I) + lr[2] * abs_r_mat(2, I);
		
			if (AbsF(t.x * r_mat(0, I) + t.y * r_mat(1, I) + t.z * r_mat(2, I)) > ra + rb)
			{
				return false;
			}
		}

		// Test the 9 different cross-axes.

		// A.x <cross> B.x
		float ra = lr.y * abs_r_mat(2, 0) + lr.z * abs_r_mat(1, 0);
		float rb = rr.y * abs_r_mat(0, 2) + rr.z * abs_r_mat(0, 1);
		if (AbsF(t.z * r_mat(1, 0) - t.y * r_mat(2, 0)) > ra + rb)
		{
			return false;
		}

		// A.x < cross> B.y
		ra = lr.y * abs_r_mat(2, 1) + lr.z * abs_r_mat(1, 1);
		rb = rr.x * abs_r_mat(0, 2) + rr.z * abs_r_mat(0, 0);
		if (AbsF(t.z * r_mat(1, 1) - t.y * r_mat(2, 1)) > ra + rb)
		{
			return false;
		}

		// A.x <cross> B.z
		ra = lr.y * abs_r_mat(2, 2) + lr.z * abs_r_mat(1, 2);
		rb = rr.x * abs_r_mat(0, 1) + rr.y * abs_r_mat(0, 0);
		if (AbsF(t.z * r_mat(1, 2) - t.y * r_mat(2, 2)) > ra + rb)
		{
			return false;
		}

		// A.y <cross> B.x
		ra = lr.x * abs_r_mat(2, 0) + lr.z * abs_r_mat(0, 0);
		rb = rr.y * abs_r_mat(1, 2) + rr.z * abs_r_mat(1, 1);
		if (AbsF(t.x * r_mat(2, 0) - t.z * r_mat(0, 0)) > ra + rb)
		{
			return false;
		}

		// A.y <cross> B.y
		ra = lr.x * abs_r_mat(2, 1) + lr.z * abs_r_mat(0, 1);
		rb = rr.x * abs_r_mat(1, 2) + rr.z * abs_r_mat(1, 0);
		if (AbsF(t.x * r_mat(2, 1) - t.z * r_mat(0, 1)) > ra + rb)
		{
			return false;
		}

		// A.y <cross> B.z
		ra = lr.x * abs_r_mat(2, 2) + lr.z * abs_r_mat(0, 2);
		rb = rr.x * abs_r_mat(1, 1) + rr.y * abs_r_mat(1, 0);
		if (AbsF(t.x * r_mat(2, 2) - t.z * r_mat(0, 2)) > ra + rb)
		{
			return false;
		}

		// A.z <cross> B.x
		ra = lr.x * abs_r_mat(1, 0) + lr.y * abs_r_mat(0, 0);
		rb = rr.y * abs_r_mat(2, 2) + rr.z * abs_r_mat(2, 1);
		if (AbsF(t.y * r_mat(0, 0) - t.x * r_mat(1, 0)) > ra + rb)
		{
			return false;
		}

		// A.z <cross> B.y
		ra = lr.x * abs_r_mat(1, 1) + lr.y * abs_r_mat(0, 1);
		rb = rr.x * abs_r_mat(2, 2) + rr.z * abs_r_mat(2, 0);
		if (AbsF(t.y * r_mat(0, 1) - t.x * r_mat(1, 1)) > ra + rb)
		{
			return false;
		}

		// A.z <cross> B.z
		ra = lr.x * abs_r_mat(1, 2) + lr.y * abs_r_mat(0, 2);
		rb = rr.x * abs_r_mat(2, 1) + rr.y * abs_r_mat(2, 0);
		if (AbsF(t.y * r_mat(0, 2) - t.x * r_mat(1, 2)) > ra + rb)
		{
			return false;
		}

		return true;
	}

	bool GIntersector::Intersect(const GBoxOriented& Box, const GVector3& Point)
	{
		GVector3 Vec = Point - Box.Center;
		GVector3 HaldDim = Box.Dimension / 2.0f;

		bool CollisionX = Vec.Dot(Box.Right) <= HaldDim.x;
		bool CollisionY = Vec.Dot(Box.Up) <= HaldDim.y;
		bool CollisionZ = Vec.Dot(Box.Front) <= HaldDim.z;

		return CollisionX && CollisionY && CollisionY;
	}

	bool GIntersector::Intersect(const GBoxAligned& Box, const GBoxOriented& Box2)
	{
		GBoxOriented OBox;
		GBoxAligned::Convert(OBox, Box);

		return Intersect(OBox, Box2);
	}

	bool GIntersector::Intersect(const GBoxAligned& Box, const GBoxAligned& Box2)
	{
		GVector3 t = Box.GetCenter() - Box2.GetCenter();
		GVector3 e = 0.5f * (Box.GetDimension() + Box2.GetDimension());
	
		bool AxisX = AbsF(t.x) <= e.x;
		bool AxisY = AbsF(t.y) <= e.y;
		bool AxisZ = AbsF(t.z) <= e.z;

		return AxisX && AxisY && AxisZ;
	}

}